#ifndef GONGJI_H_
#define GONGJI_H_
#include<iostream>
#include<iostream>
#include<geodesy/utm.h>
#include<geodesy/wgs84.h>
#include<geographic_msgs/GeoPoint.h>
#include<Eigen/Dense>
#include<ros/ros.h>
#include<cmath>
#include<serial/serial.h>
#include<boost/thread.hpp>
#include<boost/bind.hpp>
#include"gps_msgs/Gongji.h"
#include"gps_msgs/Rotation.h"
#include<nav_msgs/Odometry.h>
#include"gps_msgs/Satellite.h"
#include"gps_msgs/Satellites.h"
#include <tf2_ros/transform_broadcaster.h>

#define K1            1e-7
#define K2            1e-3
#define K3            1e2/pow(2,15)
#define K4            180/pow(2,15)
#define K5            8/pow(2,15)
#define K6            150/pow(2,15)
#define K7            100/pow(2,16)

#define PACK( __Declaration__ ) __Declaration__ __attribute__((__packed__))


PACK(
struct pkgRS422_t
{
	uint8_t header01;
	uint8_t header02;
	uint8_t header03;
	uint8_t header04;
	
	int32_t lat;
	int32_t lon;
	int32_t alt;

	int16_t Ve;
	int16_t Vn;
	int16_t Vu;
	int16_t Pitch;
	int16_t Roll;
	int16_t Yaw;

	uint8_t INS_Status;
	uint16_t GPSWeek;
	uint32_t GPSTime;

	uint8_t GNSS_Status;
	uint8_t NSV1;
	uint8_t NSV2;

	int16_t online_ax;
	int16_t online_ay;
	int16_t online_az;
	int16_t online_gx;
	int16_t online_gy;
	int16_t online_gz;
	int16_t IMU_T;

	uint16_t Accuracy_HorPositon;
	uint16_t Accuracy_Height;
	uint16_t Accuracy_HorVelocity;
	uint16_t Accuracy_UpVelocity;
	uint16_t Accuracy_HorPosture;
	uint16_t Accuracy_Yaw;

	uint8_t check_value;
 
});

static const uint8_t crc_table[] =
	{
		0x00,0x31,0x62,0x53,0xc4,0xf5,0xa6,0x97,0xb9,0x88,0xdb,0xea,0x7d,0x4c,0x1f,0x2e,
		0x43,0x72,0x21,0x10,0x87,0xb6,0xe5,0xd4,0xfa,0xcb,0x98,0xa9,0x3e,0x0f,0x5c,0x6d,
		0x86,0xb7,0xe4,0xd5,0x42,0x73,0x20,0x11,0x3f,0x0e,0x5d,0x6c,0xfb,0xca,0x99,0xa8,
		0xc5,0xf4,0xa7,0x96,0x01,0x30,0x63,0x52,0x7c,0x4d,0x1e,0x2f,0xb8,0x89,0xda,0xeb,
		0x3d,0x0c,0x5f,0x6e,0xf9,0xc8,0x9b,0xaa,0x84,0xb5,0xe6,0xd7,0x40,0x71,0x22,0x13,
		0x7e,0x4f,0x1c,0x2d,0xba,0x8b,0xd8,0xe9,0xc7,0xf6,0xa5,0x94,0x03,0x32,0x61,0x50,
		0xbb,0x8a,0xd9,0xe8,0x7f,0x4e,0x1d,0x2c,0x02,0x33,0x60,0x51,0xc6,0xf7,0xa4,0x95,
		0xf8,0xc9,0x9a,0xab,0x3c,0x0d,0x5e,0x6f,0x41,0x70,0x23,0x12,0x85,0xb4,0xe7,0xd6,
		0x7a,0x4b,0x18,0x29,0xbe,0x8f,0xdc,0xed,0xc3,0xf2,0xa1,0x90,0x07,0x36,0x65,0x54,
		0x39,0x08,0x5b,0x6a,0xfd,0xcc,0x9f,0xae,0x80,0xb1,0xe2,0xd3,0x44,0x75,0x26,0x17,
		0xfc,0xcd,0x9e,0xaf,0x38,0x09,0x5a,0x6b,0x45,0x74,0x27,0x16,0x81,0xb0,0xe3,0xd2,
		0xbf,0x8e,0xdd,0xec,0x7b,0x4a,0x19,0x28,0x06,0x37,0x64,0x55,0xc2,0xf3,0xa0,0x91,
		0x47,0x76,0x25,0x14,0x83,0xb2,0xe1,0xd0,0xfe,0xcf,0x9c,0xad,0x3a,0x0b,0x58,0x69,
		0x04,0x35,0x66,0x57,0xc0,0xf1,0xa2,0x93,0xbd,0x8c,0xdf,0xee,0x79,0x48,0x1b,0x2a,
		0xc1,0xf0,0xa3,0x92,0x05,0x34,0x67,0x56,0x78,0x49,0x1a,0x2b,0xbc,0x8d,0xde,0xef,
		0x82,0xb3,0xe0,0xd1,0x46,0x77,0x24,0x15,0x3b,0x0a,0x59,0x68,0xff,0xce,0x9d,0xac
	};


double deg2rad(const double& deg)
{
	return deg*M_PI/180.0;
}

class Gongji
{
public:
	Gongji();
	~Gongji();
	Gongji(const Gongji& obj) = delete;
	Gongji& operator=(const Gongji& obj) = delete;
	bool init();
	void startReading();
	void stopReading();

private:
	bool openSerial(const std::string& port,int baudrate);
	void closeSerial();
	void readSerialThread();
	void parseIncomingData(uint8_t* buffer,size_t len);
	void parse(const uint8_t* buffer);
	

	// static uint8_t crc8(unsigned char data, unsigned int crc)
	// {
	// 	uint8_t crc8 = (0x107 & 0xFF);
	// 	crc ^= data;
	// 	for (uint8_t i=0; i<8; i++)
	// 	{
	// 		if(crc & 0x80)
	// 		{
	// 			crc <<= 1;
	// 			crc ^= crc8;
	// 		}
	// 		else{
	// 			crc <<= 1;
	// 		}
	// 	}
	// 	return crc;
	// }

	// uint8_t Calculate_Crc8(unsigned char* buf, uint16_t len)
	// {
	// 	uint8_t crc = 0x00;
	// 	while(len--)
	// 	{
	// 		crc = crc8(*buf, crc);
	// 		if(len>0x00){
	// 			buf++;
	// 		}
	// 	}
	// 	return crc;
	// }

	
	uint8_t cal_crc_table(const uint8_t* buf,size_t len) {
		uint8_t  crc = 0x00;
	
		while (len--)
		{
			crc = crc_table[crc ^ *buf++];
		}
		return (crc);
	}



private:
	ros::Publisher m_pub_rs422;
	ros::Publisher m_pub_ll2utm;
	
	// ros::Publisher m_pub_wheel;
	
	serial::Serial *m_serial_port;
	//! shared pointer to Boost thread for listening for data from novatel
	boost::shared_ptr<boost::thread> m_read_thread_ptr;
	bool m_reading_status;  //!< True if the read thread is running, false otherwise.
	uint8_t * const m_pkg_buffer;
	
	gps_msgs::Gongji m_gps;
	// gps_msgs::Rotation m_wheel;
	bool m_is_pub_ll2utm;
	bool m_is_pub_tf;
	// bool m_is_pub_wheel;
	int  m_satelliteNum;
	int m_locationState;
	
	tf2_ros::TransformBroadcaster m_tf_br;
	std::string m_child_frame_id, m_parent_frame_id;
};

#endif
